//------------------------------------------------------------------------------
//  <copyright file="Line2D.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

namespace Microsoft.Robotics.Geometry
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Linq;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Class representing a 2-D line in point/vector notation.
    /// </summary>
    [DebuggerDisplay("<{A}, {N}>")]
    [DataContract]
    public class Line2D : GeometryElement2D
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="Line2D"/> class.
        /// </summary>
        /// <param name="a">A point on the line.</param>
        /// <param name="n">A vector pointing in the direction of the line</param>
        public Line2D(Point2D a, Vector2 n)
        {
            if (n.Length() == 0)
            {
                throw new ArgumentException("Vector n must be non-zero");
            }

            n = Vector2.Normalize(n);
            this.N = n;

            this.A = a;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="Line2D"/> class.
        /// </summary>
        /// <param name="a">A point on the line.</param>
        /// <param name="b">Another point on the line.</param>
        public Line2D(Point2D a, Point2D b)
        {
            if (a == b)
            {
                throw new ArgumentException("Need two distinct points to create a line.  Points a and b are the same");
            }

            this.N = new Vector2(b.X - a.X, b.Y - a.Y);
            this.N = Vector2.Normalize(this.N);

            this.A = a;
        }

        /// <summary>
        /// Gets or sets a vector pointing along the line
        /// </summary>
        [DataMember]
        public Vector2 N { get; protected set; }

        /// <summary>
        /// Gets or sets a point on the line
        /// </summary>
        [DataMember]
        public Point2D A { get; protected set; }

        /// <summary>
        /// Compute the distance from another geometry element to the line
        /// </summary>
        /// <param name="e">A geometry element.</param>
        /// <returns>A distance.</returns>
        public override double DistanceFrom(GeometryElement2D e)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Get the distance from the line to a point
        /// </summary>
        /// <param name="point">A point.</param>
        /// <returns>Distance from the point.</returns>
        public virtual double DistanceFrom(Point2D point)
        {
            return ((this.A - point) - (Vector2.Dot(this.A - point, this.N) * this.N)).Length();
        }

        /// <summary>
        /// Gets a point on the line computed as P = A + t*N.
        /// </summary>
        /// <param name="t">Parameter for choosing point on line.</param>
        /// <returns>A point on the line.</returns>
        public Point2D GetPoint(double t)
        {
            return this.A + t * this.N;
        }

        /// <summary>
        /// Gets a (possibly empty) list of points at which two geometry elements intersect.
        /// Only indicates where intersections occur at discrete points between the hull, not area of elements, or 
        /// intersections that form something other than a point.
        /// </summary>
        /// <param name="e"> A geometry element.</param>
        /// <returns>A list of the distinct points at which the given geometry intersects this geometry.</returns>
        public override List<Point2D> GetIntersectionPoints(GeometryElement2D e)
        {
            List<Point2D> intersectionPoints = new List<Point2D>();
            if (e is Line2D)
            {
                Line2D line = e as Line2D;
                if (line.N == this.N || -line.N == this.N)
                {
                    return intersectionPoints;
                }

                double t = (line.N.Y * (line.A.X - this.A.X) + line.N.X * (this.A.Y - line.A.Y)) /
                    (line.N.Y * this.N.X - this.N.Y * line.N.X);

                intersectionPoints.Add(this.A + t * this.N);
                return intersectionPoints;
            }
            else if (e is Circle)
            {
                Circle circle = e as Circle;

                double a = this.N.X * this.N.X + this.N.Y * this.N.Y;
                double b = 2 * ((this.A.X - circle.Center.X) * this.N.X + ((this.A.Y - circle.Center.Y) * this.N.Y));
                double c = (this.A.X - circle.Center.X) * (this.A.X - circle.Center.X) +
                    (this.A.Y - circle.Center.Y) * (this.A.Y - circle.Center.Y) - circle.Radius * circle.Radius;

                double discriminant = b * b - 4 * a * c;

                if (Math.Abs(discriminant) < MathConstants.ErrorEpsilon)
                {
                    // one intersection
                    intersectionPoints.Add(this.GetPoint(-b / (2 * a)));
                    return intersectionPoints;
                }
                else if (discriminant < 0)
                {
                    // no intersection
                    return intersectionPoints;
                }
                else
                {
                    // two intersections
                    intersectionPoints.Add(this.GetPoint((-b + Math.Sqrt(discriminant)) / (2 * a)));
                    intersectionPoints.Add(this.GetPoint((-b - Math.Sqrt(discriminant)) / (2 * a)));
                    return intersectionPoints;
                }
            }
            else
            {
                throw new NotImplementedException();
            }
        }

        /// <summary>
        /// Gets a (possibly empty) list of non-point geometries at which two geometry elements intersects.
        /// Only indicates non-point intersections between the hulls of the geometries, not intersections of areas or
        /// intersections that occur at a point only.
        /// </summary>
        /// <param name="e"> A geometry element. </param>
        /// <returns>A list of the geometries describing the intersection of the two geometries excepting point intersections.</returns>
        public override List<GeometryElement2D> GetIntersectionGeometries(GeometryElement2D e)
        {
            throw new NotImplementedException();
        }

        /// <summary>
        /// Gets a value indicating whether or not geometries intersect (without determining points or geometry of intersection).
        /// </summary>
        /// <param name="e">A 2D geometry element.</param>
        /// <returns>True if geometries intersect.</returns>
        public override bool Intersects(GeometryElement2D e)
        {
            if (e is Line2D)
            {
                List<Point2D> intersectionPoints = e.GetIntersectionPoints(this);
                if (intersectionPoints.Count > 0)
                {
                    return true;
                }

                return false;
            }
            else
            {
                throw new NotImplementedException();
            }
        }
    }
}
